#pragma once

#include "Constants.h"

inline bool fp_similar(fp32 a, fp32 b, fp32 eps = EPS_3) {
	return (a - b) < eps;
}

class Quaternion;

//	Class for basic vector with 2 int values
class Vector2i {
public:
//	ctor
	inline Vector2i() : _x(0), _y(0) {}
	inline Vector2i(int x, int y) : _x(x), _y(y) {}
//	copy ctor and assignment operator
	inline Vector2i(const Vector2i &v) { *this = v; };
	inline Vector2i& Vector2i::operator=(const Vector2i &v) { x(v.x()); y(v.y()); return *this; };

//	getters
	inline int x() const { return _x; }
	inline int y() const { return _y; }
	
//	setters
	inline void x(int value) { _x = value; }
	inline void y(int value) { _y = value; }

	inline void set(const Vector2i &vec) { set(vec.x(), vec.y()); }
	inline void set(int x, int y) { _x = x; _y = y; }
	inline void setAll(int value) { set(value, value); }

private:
	int _x;
	int _y;
};

//	Class for basic vector with 2 float values
class Vector2f {
public:
//	ctor
	inline Vector2f() : _x(0), _y(0) {}
	inline Vector2f(fp32 x, fp32 y) : _x(x), _y(y) {}
//	copy ctor and assignment operator
	inline Vector2f(const Vector2f &v) { *this = v; };
	inline Vector2f& Vector2f::operator=(const Vector2f &v) { x(v.x()); y(v.y()); return *this; };

//	getters
	inline fp32 x() const { return _x; }
	inline fp32 y() const { return _y; }
	
//	setters
	inline void x(fp32 value) { _x = value; }
	inline void y(fp32 value) { _y = value; }

	inline void set(const Vector2f &vec) { set(vec.x(), vec.y()); }
	inline void set(fp32 x, fp32 y) { _x = x; _y = y; }
	inline void setAll(fp32 value) { set(value, value); }

private:
	fp32 _x;
	fp32 _y;
};

//	Class for basic vector with 3 float coordinates
class Vector3f {
public:
//	ctor
	inline Vector3f() : _x(0), _y(0), _z(0) {}
	inline Vector3f(fp32 x, fp32 y, fp32 z) : _x(x), _y(y), _z(z) {}
//	copy ctor and assignment operator
	inline Vector3f(const Vector3f &v) { *this = v; };
	inline Vector3f& Vector3f::operator=(const Vector3f &v) { x(v.x()); y(v.y()); z(v.z()); return *this; };

//	getters
	inline fp32 x() const { return _x; }
	inline fp32 y() const { return _y; }
	inline fp32 z() const { return _z; }

	inline fp32 get(u32 pos) const {
		switch(pos) {
		case 0:
			return _x;
			break;
		case 1:
			return _y;
			break;
		case 2:
			return _z;
			break;
		default:
			NODEFAULT;
			return 0;
	};
}
	
//	setters
	inline void x(fp32 value) { _x = value; }
	inline void y(fp32 value) { _y = value; }
	inline void z(fp32 value) { _z = value; }

	inline void set(u32 pos, fp32 value)  {
		switch(pos) {
		case 0:
			_x = value;
			break;
		case 1:
			_y = value;
			break;
		case 2:
			_z = value;
			break;
		default:
			NODEFAULT;
		};
	}

	inline void set(const Vector3f &vec) { set(vec.x(), vec.y(), vec.z()); }
	inline void set(fp32 x, fp32 y, fp32 z) { _x = x; _y = y; _z = z; }
	inline void setAll(fp32 value) { set(value, value, value); }

/////////////
//	functionality
/////////////

//	addition
	static inline Vector3f add(const Vector3f v1, const Vector3f v2) { Vector3f ret(v1); ret.add(v2); return ret; }
	inline Vector3f &add(const Vector3f v) { _x += v.x(); _y += v.y(); _z += v.z(); return *this; }

//	substraction
	inline Vector3f &sub(const Vector3f v) { _x -= v.x(); _y -= v.y(); _z -= v.z(); return *this; }

//	multiplication
	static inline Vector3f mul(const Vector3f &vec, const fp32 v) { return Vector3f(vec.x() * v, vec.y() * v, vec.z() * v); }
	inline Vector3f &mul(const fp32 v) { _x *= v; _y *= v; _z *= v;  return *this; }

//	division
	inline Vector3f &div(const fp32 v) { _x /= v; _y /= v; _z /= v;  return *this; }

//	length
	inline fp32 len() { return sqrtf(_x*_x + _y*_y + _z*_z); }

//	normalization
	inline Vector3f &nrm() { div(len());  return *this; }

//	dot product
	fp32 dot(const Vector3f &r) { 
		// NOT IMPLEMENTED!
		verify(false);
		return 0.f;
	}
	static fp32 dot(const Vector3f &l, const Vector3f &r) { 
		return l.x() * r.x() + l.y() * r.y() + l.z() * r.z();
	}

//	cross product
	Vector3f &cross(const Vector3f &r) { 
		x(y() * r.z() - z() * r.y());
		y(z() * r.x() - x() * r.z());
		z(x() * r.y() - y() * r.x());
		return *this;
	}

	static Vector3f cross(const Vector3f &l, const Vector3f &r) { 
		const fp32 x = l.y() * r.z() - l.z() * r.y();
		const fp32 y = l.z() * r.x() - l.x() * r.z();
		const fp32 z = l.x() * r.y() - l.y() * r.x();
		return Vector3f(x, y, z);
	}

//	rotation
	Vector3f &rotate(fp32 angle, const Vector3f &axis);

private:
//	data
	fp32 _x, _y, _z;
};

//	Class for basic vector with 4 coordinates
class Vector4f {
public:
//	ctor
	inline Vector4f() : _w(0) {}
	inline Vector4f(fp32 x, fp32 y, fp32 z, fp32 w) : _vec(Vector3f(x, y, z)), _w(w) {}
//	copy ctor and assignment operator
	inline Vector4f(const Vector4f &v) { *this = v; };
	inline Vector4f& Vector4f::operator=(const Vector4f &v) { _vec = v.vec3(); w(v.w()); return *this; };

//	getters
	inline fp32 x() const { return _vec.x(); }
	inline fp32 y() const { return _vec.y(); }
	inline fp32 z() const { return _vec.z(); }
	inline fp32 w() const { return _w; }

	inline fp32 get(u32 pos) const {
		if (pos == 4) {
			return _w;
		} else {
			return _vec.get(pos);
		}
	}

	inline const Vector3f &vec3() const { return _vec; }

//	setters
	inline void x(fp32 value) { _vec.x(value); }
	inline void y(fp32 value) { _vec.y(value); }
	inline void z(fp32 value) { _vec.z(value); }
	inline void w(fp32 value) { _w = value; }

	inline void set(u32 pos, fp32 value) {
		if (pos == 4) {
			_w = value;
		} else {
			_vec.set(pos, value);
		}
	}

	inline void set(const Vector4f &vec) { set(vec.x(), vec.y(), vec.z(), vec.w()); }
	inline void set(fp32 x, fp32 y, fp32 z, fp32 w) { _vec.set(x, y, z); _w = w; }
	inline void setAll(fp32 value) { set(value, value, value, value); }

/////////////
//	functionality
/////////////

//	addition
	inline Vector4f &add(const Vector4f v) { _vec.add(v.vec3()); _w += v.w(); return *this; }

//	substraction
	inline Vector4f &sub(const Vector4f v) { _vec.sub(v.vec3()); _w -= v.w(); return *this; }

	//	multiplication
	inline Vector4f &mul(const fp32 v) { _vec.mul(v); _w *= v;  return *this; }

//	division
	inline Vector4f &div(const fp32 v) { _vec.div(v); _w /= v;  return *this; }

//	length
	// ??? is this right ??? what _w stands for???
//	inline fp32 len() { return sqrtf(_vec.x()*_vec.x() + _vec.y()*_vec.y() + _vec.z()*_vec.z() + _w*_w); }
	inline fp32 len3() { return _vec.len(); }
	
//	normalization
//	inline Vector4f &nrm() { div(len()); return *this; }
	inline Vector4f &nrm3() { _vec.div(len3()); return *this; }

private:

private:
//	data
	Vector3f _vec;
	fp32 _w;
};

class Matrix_4x4 {
public:
//	ctor
	inline Matrix_4x4() {
		setIdentity();
	}

//	copy ctor and assignment operator
	inline Matrix_4x4(const Matrix_4x4 &v) { *this = v; };
	inline Matrix_4x4 &Matrix_4x4::operator=(const Matrix_4x4 &v) { set(v); return *this; };

//	getters
	inline fp32 get(u32 pos_i, u32 pos_j) const { return _v[pos_i][pos_j]; }
	inline fp32 &get(u32 pos_i, u32 pos_j) { return _v[pos_i][pos_j]; }
	inline fp32 &get0() { return _v[0][0]; }
	inline Vector3f position() const { return Vector3f(get(3,0), get(3,1), get(3,2)); }

//	setters
	inline void set(u32 pos_i, u32 pos_j, fp32 value) { _v[pos_i][pos_j] = value; }
	inline Matrix_4x4 &set(const Matrix_4x4 &m) { memcpy(this, &m, sizeof(Matrix_4x4)); return *this; }
	inline void set(const fp32 vec[16]) { memcpy(_v, &vec, sizeof(vec)); }

/////////////
//	functionality
/////////////
	inline Matrix_4x4 &setIdentity() { 
		setZero();
		set(0, 0, 1.f);
		set(1, 1, 1.f);
		set(2, 2, 1.f);
		set(3, 3, 1.f);
		return *this;
	}

	inline Matrix_4x4 &setZero() { 
		memset(this, 0, sizeof(Matrix_4x4));
		return *this;
	}

	inline Matrix_4x4 &translate(const Vector3f pos) {
		set(0, 3, pos.x());
		set(1, 3, pos.y());
		set(2, 3, pos.z());
		return *this;
	}

	inline Matrix_4x4 &rotate(const Vector3f angles_deg) {
		Matrix_4x4 rx, ry, rz;

		const fp32 x = TO_RADIAN(angles_deg.x());
		const fp32 y = TO_RADIAN(angles_deg.y());
		const fp32 z = TO_RADIAN(angles_deg.z());

		rx.setIdentity();
		rx.set(1, 1, cosf(x));
		rx.set(1, 2, -sinf(x));
		rx.set(2, 1, sinf(x));
		rx.set(2, 2, cosf(x));

		ry.setIdentity();
		ry.set(0, 0, cosf(y));
		ry.set(0, 2, -sinf(y));
		ry.set(2, 0, sinf(y));
		ry.set(2, 2, cosf(y));

		rz.setIdentity();
		rz.set(0, 0, cosf(z));
		rz.set(0, 1, -sinf(z));
		rz.set(1, 0, sinf(z));
		rz.set(0, 0, cosf(z));

		set(Matrix_4x4().mulb(rz).mulb(ry).mulb(rx));

		return *this;
	}

	inline Matrix_4x4 &scale(const Vector3f &scale_vec) {
		set(0, 0, scale_vec.x());
		set(1, 1, scale_vec.y());
		set(2, 2, scale_vec.z());
		return *this;
	}

	static inline Matrix_4x4 projectionMatrix(const PerspectiveParams &proj_params) {
		const fp32 ar = (float)proj_params.width / (float)proj_params.height;
		const fp32 znear_v = proj_params.near_v;
		const fp32 zfar_v = proj_params.far_v;
		const fp32 zRange = znear_v - zfar_v;
		const fp32 tanHalfFOV = tanf(TO_RADIAN(proj_params.fov / 2.0));

		Matrix_4x4 proj;
		proj.setZero();
		proj.set(0, 0, 1.0f / (tanHalfFOV * ar));
		proj.set(1, 1, 1.0f / tanHalfFOV);
		proj.set(2, 2, (-znear_v - zfar_v) / zRange);
		proj.set(2, 3, 2.0f * zfar_v * znear_v / zRange);
		proj.set(3, 2, 1.f);

		return proj;
	}

	inline Matrix_4x4 &mula(const Matrix_4x4 &l) {
		Matrix_4x4 ret;
		for (u32 i = 0; i < 4; ++i) {
			for (u32 j = 0; j < 4; ++j) {
				ret.set(i, j,	l.get(i, 0) * get(0, j) + 
								l.get(i, 1) * get(1, j) + 
								l.get(i, 2) * get(2, j) + 
								l.get(i, 3) * get(3, j));
			}
		}

		return set(ret);
	}

	inline Matrix_4x4 &mulb(const Matrix_4x4 &r) {
		Matrix_4x4 ret;
		for (u32 i = 0; i < 4; ++i) {
			for (u32 j = 0; j < 4; ++j) {
				ret.set(i, j,	get(i, 0) * r.get(0, j) + 
								get(i, 1) * r.get(1, j) + 
								get(i, 2) * r.get(2, j) + 
								get(i, 3) * r.get(3, j));
			}
		}

		return set(ret);
	}

	inline static Matrix_4x4 mul(const Matrix_4x4 &l, const Matrix_4x4 &r) {
		Matrix_4x4 matrix;
		for (u32 i = 0; i < 4; ++i) {
			for (u32 j = 0; j < 4; ++j) {
				matrix.set(i, j,	l.get(i, 0) * r.get(0, j) + 
									l.get(i, 1) * r.get(1, j) + 
									l.get(i, 2) * r.get(2, j) + 
									l.get(i, 3) * r.get(3, j));
			}
		}
		return matrix;
	}

	inline static Matrix_4x4 buildTransform(const Vector3f scale, const Vector3f rotation, const Vector3f translation) {
		Matrix_4x4 s;
		s.scale(scale);
		Matrix_4x4 r;
		r.rotate(rotation);
		Matrix_4x4 t;
		t.translate(translation);

		return t.mulb(r).mulb(s);
	}

private:
	fp32 _v[4][4];
};

// Quaternion class
// TODO: Maybe should inherit Vector4f and override functionality?...
class Quaternion {
public:
//	ctor
	inline Quaternion() : _x(0), _y(0), _z(0), _w(0) {}
	inline Quaternion(fp32 x, fp32 y, fp32 z, fp32 w) : _x(x), _y(y), _z(z), _w(w) {}
//	copy ctor and assignment operator
	inline Quaternion(const Quaternion &v) { *this = v; };
	inline Quaternion& Quaternion::operator=(const Quaternion &v) { x(v.x()); y(v.y()); z(v.z()); w(v.w()); return *this; };

//	getters
	inline fp32 x() const { return _x; }
	inline fp32 y() const { return _y; }
	inline fp32 z() const { return _z; }
	inline fp32 w() const { return _w; }

//	setters
	inline void x(fp32 value) { _x = value; }
	inline void y(fp32 value) { _y = value; }
	inline void z(fp32 value) { _z = value; }
	inline void w(fp32 value) { _w = value; }

	inline void set(const Quaternion &q) { set(q.x(), q.y(), q.z(), q.w()); }
	inline void set(fp32 x, fp32 y, fp32 z, fp32 w) { _x = x; _y = y; _z = z; _w = w; }
	inline void setAll(fp32 value) { set(value, value, value, value); }

/////////////
//	functionality
/////////////

	inline void nrm() {
		fp32 length = sqrtf(_x * _x + _y * _y + _z * _z + _w * _w);
		_x /= length;
		_y /= length;
		_z /= length;
		_w /= length;
	}

	inline Quaternion conjugate() const {
		return Quaternion(-_x, -_y, -_z, _w);
	}

private:
	fp32 _x;
	fp32 _y;
	fp32 _z;
	fp32 _w;
};

inline Quaternion operator*(const Quaternion &l, const Quaternion &r) {
	const fp32 w = (l.w() * r.w()) - (l.x() * r.x()) - (l.y() * r.y()) - (l.z() * r.z());
	const fp32 x = (l.x() * r.w()) + (l.w() * r.x()) + (l.y() * r.z()) - (l.z() * r.y());
	const fp32 y = (l.y() * r.w()) + (l.w() * r.y()) + (l.z() * r.x()) - (l.x() * r.z());
	const fp32 z = (l.z() * r.w()) + (l.w() * r.z()) + (l.x() * r.y()) - (l.y() * r.x());

	return Quaternion(x, y, z, w);
}

inline Quaternion operator*(const Quaternion &q, const Vector3f &v) {
	const fp32 w = - (q.x() * v.x()) - (q.y() * v.y()) - (q.z() * v.z());
	const fp32 x = (q.w() * v.x()) + (q.y() * v.z()) - (q.z() * v.y());
	const fp32 y = (q.w() * v.y()) + (q.z() * v.x()) - (q.x() * v.z());
	const fp32 z = (q.w() * v.z()) + (q.x() * v.y()) - (q.y() * v.x());

	return Quaternion(x, y, z, w);
}